#include "Henning_PID.h"
#include "WPILib.h"

PID::PID(float p, float i, float d, float max_output, float max_accum_error) {
	m_p = p;
	m_i = i;
	m_d = d;
	m_maxOutput = max_output;
	m_maxAccumError = max_accum_error;
	m_accumError = 0;
	m_prevError = 0;
}

float PID::GetOutput(float current, float target) {
	float output, error, change_in_error;
	
	error = current - target;
	change_in_error = m_prevError - error;
	m_accumError += error;
	if (m_accumError > m_maxAccumError) m_accumError = m_maxAccumError;
	if (m_accumError < -m_maxAccumError) m_accumError = -m_maxAccumError;
	m_prevError = error;
	
	//printf("PID(p=%f, i=%f, d=%f) = %f\n",(error * m_p), (m_accumError * m_i), (change_in_error * m_d), (error * m_p) + (m_accumError * m_i) + (change_in_error * m_d));
	output = (error * m_p) + (m_accumError * m_i) + (change_in_error * m_d);
	if (output > m_maxOutput) output = m_maxOutput;
	if (output < -m_maxOutput) output = -m_maxOutput;
	
	return output;
}
